#include "mainwindow.h"
#include <QString>

#include <QDebug>

hduVector3Dd position;
hduVector3Dd force = { 0, 0, 0 };
hduVector3Dd JointEncoders;
hduVector3Dd GimbalEncoders;
hduVector3Dd anchor;
HDboolean bRenderForce = HD_FALSE;
// 主窗口构造函数
MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    // 设置 UI 界面
    ui->setupUi(this);
    // 初始化 QTimer
    timer = new QTimer(this);
    // 连接 timeout 信号到 updatePositionLabel 槽函数
    connect(timer, &QTimer::timeout, this, [this]() {
        updatePositionLabel(position, JointEncoders, GimbalEncoders,force);
    });
    // 设置定时器间隔为 200 毫秒
    timer->setInterval(200);
    // 启动定时器
    timer->start();
    // 初始化弹簧刚度
    gSpringStiffness = 0.1;


    HDErrorInfo error;
    // 初始化触觉设备，获取设备句柄
    hHD = hdInitDevice(HD_DEFAULT_DEVICE);
    // 检查设备初始化是否出错
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        // 打印错误信息到标准错误输出
        hduPrintError(stderr, &error, "Failed to initialize haptic device");
        // 在调试输出中提示用户按任意键退出
        qDebug() << "Press any key to quit.";
        return;
    }

    // 在调试输出中显示示例名称
    qDebug() << "Anchored Spring Force Example";

    // 安排异步回调函数，用于渲染力和更新位置信息
    gCallbackHandle = hdScheduleAsynchronous(
        [](void *pUserData) -> HDCallbackCode {


            HDErrorInfo error;

            HDint nCurrentButtons, nLastButtons;

            // 开始一帧的力渲染
            hdBeginFrame(hdGetCurrentDevice());

            // 获取当前设备的位置
            hdGetDoublev(HD_CURRENT_POSITION, position);
            hdGetDoublev(HD_CURRENT_JOINT_ANGLES, JointEncoders);
            hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, GimbalEncoders);
            // 获取当前按钮状态
            hdGetIntegerv(HD_CURRENT_BUTTONS, &nCurrentButtons);
            // 获取上一帧按钮状态
            hdGetIntegerv(HD_LAST_BUTTONS, &nLastButtons);



            // 标记开始渲染力
            // 检测按钮按下事件
            if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 &&
                (nLastButtons & HD_DEVICE_BUTTON_1) == 0)
            {
                // 记录锚点位置
                memcpy(anchor, position, sizeof(hduVector3Dd));
                // 标记开始渲染力
                bRenderForce = HD_TRUE;
            }
            // 检测按钮释放事件
            else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 &&
                     (nLastButtons & HD_DEVICE_BUTTON_1) != 0)
            {
                // 标记停止渲染力
                bRenderForce = HD_FALSE;
                // 发送零力到设备
                force = { 0, 0, 0 };
                hdSetDoublev(HD_CURRENT_FORCE, force);
            }

            // 如果需要渲染力
            if (bRenderForce)
            {
                // 计算弹簧力 F = k * (anchor - pos)
                hduVecSubtract(force, anchor, position);
                // 缩放力向量，乘以当前弹簧刚度
                hduVecScaleInPlace(force, static_cast<MainWindow*>(pUserData)->gSpringStiffness);
                // 设置当前力
                hdSetDoublev(HD_CURRENT_FORCE, force);
            }


            // 结束一帧的力渲染
            hdEndFrame(hdGetCurrentDevice());



            // 检查是否有错误发生
            if (HD_DEVICE_ERROR(error = hdGetError()))
            {
                // 如果是力渲染错误，停止渲染力
                if (hduIsForceError(&error))
                {
                    bRenderForce = HD_FALSE;
                }
                // 如果是调度器错误，结束回调
                else if (hduIsSchedulerError(&error))
                {
                    return HD_CALLBACK_DONE;
                }
            }

            // 继续执行回调
            return HD_CALLBACK_CONTINUE;
        }, this, HD_MAX_SCHEDULER_PRIORITY);

    // 启用力输出
    hdEnable(HD_FORCE_OUTPUT);
    // 获取设备支持的最大刚度
    //hdGetDoublev(HD_NOMINAL_MAX_STIFFNESS, &gMaxStiffness);
    // 启动调度器
    hdStartScheduler();
    // 检查调度器启动是否出错
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        // 打印错误信息到标准错误输出
        hduPrintError(stderr, &error, "Failed to start scheduler");
        // 在调试输出中提示用户按任意键退出
        qDebug() << "Press any key to quit.";
        return;
    }


}

// 主窗口析构函数
MainWindow::~MainWindow()
{
    // 停止调度器
    hdStopScheduler();
    // 取消异步回调
    hdUnschedule(gCallbackHandle);
    // 禁用触觉设备
    hdDisableDevice(hHD);
    // 释放 UI 界面指针
    delete ui;
}



// 退出按钮的点击事件处理函数
void MainWindow::on_quitButton_clicked()
{
    // 关闭主窗口
    close();
}

// 更新位置标签文本的函数
void MainWindow::updatePositionLabel(const hduVector3Dd& position, const hduVector3Dd& JointEncoders, const hduVector3Dd& GimbalEncoders,const hduVector3Dd& force)
{

        // 将弧度制转换为角度值
    hduVector3Dd roundedPosition;
    roundedPosition[0] = position[0];
    roundedPosition[1] = position[1];
    roundedPosition[2] = position[2];
    hduVector3Dd roundedforce;
    roundedforce[0] = force[0];
    roundedforce[1] = force[1];
    roundedforce[2] = force[2];
    hduVector3Dd angleJointEncoders;
    angleJointEncoders[0] = JointEncoders[0] * (180.0 / 3.1415926);
    angleJointEncoders[1] = JointEncoders[1] * (180.0 / 3.1415926);
    angleJointEncoders[2] = JointEncoders[2] * (180.0 / 3.1415926);

    hduVector3Dd angleGimbalEncoders;
    angleGimbalEncoders[0] = GimbalEncoders[0] * (180.0 / 3.1415926);
    angleGimbalEncoders[1] = GimbalEncoders[1] * (180.0 / 3.1415926);
    angleGimbalEncoders[2] = GimbalEncoders[2] * (180.0 / 3.1415926);

    // 保留两位小数并格式化为字符串
    QString positionText = QString("主手位置: X:%1, Y:%2, Z:%3")
                               .arg(roundedPosition[0], 0, 'f', 2)
                               .arg(roundedPosition[1], 0, 'f', 2)
                               .arg(roundedPosition[2], 0, 'f', 2);

    QString angle1Text = QString("1:%1, 2:%2, 3:%3")
                             .arg(angleJointEncoders[0], 0, 'f', 2)
                             .arg(angleJointEncoders[1], 0, 'f', 2)
                             .arg(angleJointEncoders[2], 0, 'f', 2);

    QString angle2Text = QString("4:%1, 5:%2, 6:%3")
                             .arg(angleGimbalEncoders[0], 0, 'f', 2)
                             .arg(angleGimbalEncoders[1], 0, 'f', 2)
                             .arg(angleGimbalEncoders[2], 0, 'f', 2);
    QString force1Text = QString("主手力反馈X:%1, Y:%2, Z:%3")
                             .arg(roundedforce[0], 0, 'f', 2)
                             .arg(roundedforce[1], 0, 'f', 2)
                             .arg(roundedforce[2], 0, 'f', 2);
    // 更新位置标签的文本
    ui->positionLabel->setText(positionText);
    ui->angle1->setText(angle1Text);
    ui->angle2->setText(angle2Text);
    ui->force1->setText(force1Text);


}
